#include <BluetoothSerial.h>

BluetoothSerial SerialBT;
char serialData;

int IN1 = 4;
int IN2 = 5;
int IN3 = 18;
int IN4 = 19;

int IN5 = 12;
int IN6 = 14;
int IN7 = 27;
int IN8 = 26;

int X1 = 25;
int X2 = 33;
int X3 = 23;
int X4 = 35;
int X5 = 34;

int PWM1 = 15; 
int PWM2 = 13;

void setup() 
{
    SerialBT.begin("遥控3号");
    Serial.begin(9600);
    
    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);

    pinMode(IN5, OUTPUT);
    pinMode(IN6, OUTPUT);
    pinMode(IN7, OUTPUT);
    pinMode(IN8, OUTPUT);

    pinMode(X1, INPUT);    
    pinMode(X2, INPUT);
    pinMode(X3, INPUT);
    pinMode(X4, INPUT);
    pinMode(X5, INPUT);
        
    ledcSetup(0, 5000, 8);  
    ledcSetup(1, 5000, 8);        
    ledcAttachPin(PWM1, 0);
    ledcAttachPin(PWM2, 1);
    

}
  
void loop() 
{
      //遥控
      Xunji();
}


   
void LF_forward() //左前轮前进
{     
      digitalWrite(IN1,LOW);
      digitalWrite(IN2,HIGH);
}

 void LF_backward()//左前轮后退
{      
      digitalWrite(IN1,HIGH);
      digitalWrite(IN2,LOW);
}
  
void LF_stop() //左前轮停止
{     
      digitalWrite(IN1,LOW);
      digitalWrite(IN2,LOW);
}

void RF_forward()//右前轮前进
{      
      digitalWrite(IN3,LOW);
      digitalWrite(IN4,HIGH);
}

void RF_backward()//右前轮后退
{    
      digitalWrite(IN3,HIGH);
      digitalWrite(IN4,LOW);
}  

void RF_stop()//右前轮停止
{    
      digitalWrite(IN3,LOW);
      digitalWrite(IN4,LOW);
}  

void LB_forward() //左后轮前进
{
        digitalWrite(IN5,HIGH);
        digitalWrite(IN6,LOW);
}

void LB_backward() //左后轮后退
{
        digitalWrite(IN5,LOW);
        digitalWrite(IN6,HIGH);
}

void LB_stop() //左后轮停止
{
        digitalWrite(IN5,LOW);
        digitalWrite(IN6,LOW);
}

void RB_forward() //右后轮前进
{
        digitalWrite(IN7,HIGH);
        digitalWrite(IN8,LOW);
}

void RB_backward() //右后轮后退
{
        digitalWrite(IN7,LOW);
        digitalWrite(IN8,HIGH);
}

void RB_stop() //右后轮停止
{
        digitalWrite(IN7,LOW);
        digitalWrite(IN8,LOW);
}

void Run()     //前进
{
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  LF_forward(); //LF前进
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_forward(); //RB前进
}

void Run_Y()     //前进
{
  LF_forward(); //LF前进
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_forward(); //RB前进
}

void Back()    //后退
{  
  LF_backward(); //LF后退
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_backward(); //RB后退
}

void Stop()   //停止
{
  LF_stop(); //LF停止
  RF_stop(); //RF停止
  LB_stop(); //LB停止
  RB_stop(); //RB停止
}

void Left()   //左移
{
  LF_backward(); //LF后退
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_backward(); //RB后退
}

void Left_up()  //左前移
{ 
  LF_stop(); //LF停止
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_stop(); //RB停止
}

void Left_down()  //左后移
{ 
  LF_backward(); //LF后退
  RF_stop(); //RF停止
  LB_stop(); //LB停止
  RB_backward(); //RB后退
}

void Right_right()  //右拐
{
  LF_forward(); //LF后退
  RF_backward(); //RF前进
  LB_forward(); //LB前进
  RB_backward(); //RB后退
}

void Right()  //右移
{
  LF_forward(); //LF前进
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_forward(); //RB前进
}

void Right_up()  //右前移
{ 
  LF_forward(); //LF前进
  RF_stop(); //RF后退
  LB_stop(); //LB后退
  RB_forward(); //RB前进
}

void Right_down()   //右后移
{
  LF_stop(); //LF前进
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_stop(); //RB前进
}

void Left_left()  //左拐
{
  LF_backward(); //LF前进
  RF_forward(); //RF后退
  LB_backward(); //LB后退
  RB_forward(); //RB前进
}

void Run_right()     //右前拐
{
  LF_forward(); //LF前进
  LB_forward(); //LB前进
  RF_stop(); //RF停止
  RB_stop(); //RB停止
}

void Run_left()     //左前拐
{
  LF_stop(); //LF停止
  LB_stop(); //LB停止
  RF_forward(); //RF前进
  RB_forward(); //RB前进
}

void Back_right()    //右后拐
{
  LF_backward(); //LF后退
  LB_backward(); //LB后退 
  RF_stop(); //RF停止
  RB_stop(); //RB停止
}

void Back_left()    //左后拐
{
  LF_stop(); //LF停止
  LB_stop(); //LB停止 
  RF_backward(); //RF后退
  RB_backward(); //RB后退
}

void S_left()    //小左
{
  ledcWrite(0, 0);
  ledcWrite(1, 150);
  LF_forward(); //LF前进
  LB_forward(); //LB前进 
  RF_forward(); //RF前进
  RB_forward(); //RB前进
}

void B_left()    //大左
{
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  LF_backward(); //LF后退
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_backward(); //RB后退
}

void L_left()    //急左
{
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  LF_backward(); //LF后退
  RF_forward(); //RF前进
  LB_forward(); //LB前进
  RB_backward(); //RB后退
}

void S_right()    //小右
{
  ledcWrite(0, 150);
  ledcWrite(1, 0);
  LF_forward(); //LF前进
  LB_forward(); //LB前进 
  RF_forward(); //RF前进
  RB_forward(); //RB前进
}

void B_right()    //大右
{
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  LF_forward(); //LF前进
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_forward(); //RB前进
}

void R_right()    //急右
{
  ledcWrite(0, 150);
  ledcWrite(1, 150);
  LF_forward(); //LF前进
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_forward(); //RB前进
}
void S_Back()    //后退
{  
  ledcWrite(0, 150);
  ledcWrite(1, 125);

  LF_backward(); //LF后退
  RF_backward(); //RF后退
  LB_backward(); //LB后退
  RB_backward(); //RB后退
}

void  Yaokong()  //遥控模式
{
    ledcWrite(0, 200);
    ledcWrite(1, 200);
  
  if( SerialBT.available()>0 )
    {
      serialData =  SerialBT.read();
      if (serialData == '8' )
      {
           //前进
           Run_Y();
      }
      else if(serialData == '2' )
      {
         //后退
         Back();
      }
      else if(serialData == '6' )
      {
         //右移
         Right();
      }
      else if(serialData == '4' )
      {
        //左移
        Left();
      }
      else if(serialData == '7' )
      {
        //左前移
        Left_up();
      } 
       else if(serialData == '1' )
      {
        //左后移
        Left_down();
      }
      else if(serialData == 'a' )
      {
        //左转
        Left_left();
      }
       else if(serialData == '9' )
      {
         //右前移
         Right_up();
      }
       else if(serialData == '3' )
      {
         //右后移
         Right_down();
      }
      else if(serialData == 'd' )
      {
         //右转
         Right_right();
      } 
      else if(serialData == 'q' )
      {
        //左前转
         Run_left();
      } 
       else if(serialData == 'c' )
      {
        //右后转
        Back_left();
      }
      
       else if(serialData == 'e' )
      {
         //右前转
         Run_right();
      }
       else if(serialData == 'z' )
      {
         //左后转
         Back_right();
      }  
      else
      {
         //停止
        Stop(); 
      }
   }
}

void Xunji()  //循迹模式
{
    int M1 = digitalRead(X1);
    int M2 = digitalRead(X2);
    int M3 = digitalRead(X3);
    int M4 = digitalRead(X4);
    int M5 = digitalRead(X5);
    Serial.print("M1=");
    Serial.print(M1);
    Serial.print("M2=");
    Serial.print(M2);
    Serial.print("M3=");
    Serial.print(M3);
    Serial.print("M4=");
    Serial.print(M4);
    Serial.print("M5=");
    Serial.print(M5);
    Serial.println();
        
        if(M1 == HIGH && M2 == HIGH && M3 == LOW && M4 == HIGH && M5 == HIGH)
        {
              Run();
        } 
        else if(M1 == HIGH && M2 == LOW && M3 == HIGH && M4 == HIGH && M5 == HIGH)
        {
               S_left();
        }
        else if(M1 == HIGH && M2 == LOW && M3 == LOW && M4 == HIGH && M5 == HIGH)
        {
               S_left();
        }
        else if(M1 == LOW && M2 == HIGH && M3 == HIGH && M4 == HIGH && M5 == HIGH)
        {
               L_left();
        }
        else if(M1 == LOW && M2 == LOW && M3 == HIGH && M4 == HIGH && M5 == HIGH)
        {
               L_left();
        }
        else if(M1 == LOW && M2 == LOW && M3 == LOW && M4 == HIGH && M5 == HIGH)
        {
               L_left();
        }
        else if(M1 == HIGH && M2 == HIGH && M3 == HIGH && M4 == LOW && M5 == HIGH)
        {
               S_right();
        }
        else if(M1 == HIGH && M2 == HIGH && M3 == LOW && M4 == LOW && M5 == HIGH)
        {
               S_right();
        }
        else if(M1 == HIGH && M2 == HIGH && M3 == HIGH && M4 == HIGH && M5 == LOW)
        {
               R_right();
        } 
        else if(M1 == HIGH && M2 == HIGH && M3 == HIGH && M4 == LOW && M5 == LOW)
        {
               R_right();
        }
        else if(M1 == HIGH && M2 == HIGH && M3 == LOW && M4 == LOW && M5 == LOW)
        {
               R_right();
        }
        else if(M1 == HIGH && M2 == HIGH && M3 == HIGH && M4 == HIGH && M5 == HIGH)
        {
            S_Back();
        }
        else if(M1 == LOW && M2 == LOW && M3 == LOW && M4 == LOW && M5 == LOW)
        {
            Stop();
        }
        else
        {
          Stop();
        }
}
